from robot_arm_module import RobotArmModule

def main():
    # 创建机械臂实例 L1=114, L2=60, 末端负载1g
    arm = RobotArmModule(l1=114, l2=60, mass=0.001)
    
    # 示例1：使用正常范围内的角度
    theta1 = 30  # 度
    theta2 = -45  # 度
    torque1, torque2 = arm.calculate_torques(theta1, theta2)
    print(f"示例1 - 正常角度:")
    print(f"输入角度: theta1={theta1}°, theta2={theta2}°")
    print(f"关节力矩: 关节1={torque1:.6f} N·m, 关节2={torque2:.6f} N·m\n")
    
    # 示例2：使用超出360度的角度
    theta1 = 400  # 相当于40度
    theta2 = -405  # 相当于-45度
    # 先查看标准化后的角度
    norm_theta1 = arm.normalize_angle(theta1)
    norm_theta2 = arm.normalize_angle(theta2)
    torque1, torque2 = arm.calculate_torques(theta1, theta2)
    print(f"示例2 - 超出360度的角度:")
    print(f"输入角度: theta1={theta1}°, theta2={theta2}°")
    print(f"标准化后: theta1={norm_theta1}°, theta2={norm_theta2}°")
    print(f"关节力矩: 关节1={torque1:.6f} N·m, 关节2={torque2:.6f} N·m\n")
    
    # 示例3：结合逆运动学使用
    target_x = 120
    target_y = 40
    # 计算逆运动学（肘上解）
    angles = arm.inverse_kinematics(target_x, target_y, solution_type=0)
    if angles:
        theta1_ik, theta2_ik = angles
        torque1, torque2 = arm.calculate_torques(theta1_ik, theta2_ik)
        print(f"示例3 - 结合逆运动学:")
        print(f"目标位置: ({target_x}, {target_y})")
        print(f"逆解角度: theta1={theta1_ik:.2f}°, theta2={theta2_ik:.2f}°")
        print(f"关节力矩: 关节1={torque1:.6f} N·m, 关节2={torque2:.6f} N·m\n")
    
    # 示例4：改变负载质量
    arm.set_load_mass(0.01)  # 设置为10g
    torque1, torque2 = arm.calculate_torques(theta1, theta2)
    print(f"示例4 - 改变负载质量:")
    print(f"负载质量: 10g")
    print(f"关节力矩: 关节1={torque1:.6f} N·m, 关节2={torque2:.6f} N·m")

if __name__ == "__main__":
    main()
